#include "NLSaturation.h"
#include "Utils.h"
#include <sstream>

#include <cmath>
#include <limits>

// Przestrzeń nazw
namespace RafMat
{
namespace SimEngine
{
using namespace std;
using namespace SimInterface;
const std::string NLSaturation::CONF_ALL = "*";
const std::string NLSaturation::CONF_WORKING_POINT = "WorkingPoint";
const std::string NLSaturation::CONF_NONLINEARITY_GAIN = "NonlinearityGain";

NLSaturation::NLSaturation(const SimInterface::ISimulator &isim, double workingPoint, double nonlinearityGain)
    :NonlinearityBase(isim, "NLSaturation"),
      m_WorkingPoint(workingPoint),
      m_NonlinearityGain(nonlinearityGain)
{
}

double NLSaturation::simulateNonlinearityStep(double objectOutput)
{
    double atanArg = objectOutput - m_WorkingPoint;
    return (atan(atanArg)/M_PI_2) + m_NonlinearityGain*(atanArg);
}

void NLSaturation::reset()
{
    m_WorkingPoint = 0;
    m_NonlinearityGain = 0.2;
}


bool NLSaturation::setParameter(const std::string& param, const std::string& value) throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    std::istringstream istr(tvalue);

    double tmpVal;
    istr >> tmpVal;

    if(istr.bad())
        throw iconf_error::syntaxError(*this, tvalue);

    if(tparam == CONF_ALL)
        safeSetParameters(tvalue);
    else if(tparam == CONF_WORKING_POINT)
        m_WorkingPoint = tmpVal;
    else if(tparam == CONF_NONLINEARITY_GAIN)
        m_NonlinearityGain = tmpVal;
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    return true;
}

void NLSaturation::safeSetParameters(const std::string& values)
{
    // Zapis dający silną gwarancję bezpieczeństwa.
    // Wszystkie parametry są zapisane w formie wektora
    // (jak wielomiany). Podajemy N i Td, a i b muszą być przeliczone.
    // Kolejność jest następująca!
//    double m_k;
//    double m_b;
//    double m_ti;
//    double m_td;
//    double m_N;
//    bool m_forwardDiff;
//    double m_upperBound;
//    double m_lowerBound;

    bool succeed;

    vector<double> vect = Utils::convertStrToVector(values, succeed);

    if(!succeed)
        throw iconf_error::syntaxError(*this, values);

    if(vect.size() != 2)
        throw iconf_error(*this, values, "Saturation nonlinearity has exactly 2 parameters to set.");

    // Poniższe instrukcje muszą się udać:
    m_WorkingPoint = vect[0];
    m_NonlinearityGain = vect[1];
}

std::string NLSaturation::getParameter(const std::string& param) const throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string outStr;

    // Strumień danych dla łańcucha argumentu.
    std::ostringstream ostr(outStr);

    if(tparam == CONF_WORKING_POINT)
    {
        ostr << m_WorkingPoint;
    }
    else if(tparam == CONF_NONLINEARITY_GAIN)
    {
        ostr << m_NonlinearityGain;
    }
    else
        return NonlinearityBase::getParameter(tparam);

    return ostr.str();
}


SimFile::ConfigWriter& NLSaturation::dumpSpecificPar(SimFile::ConfigWriter& o) const
{
    o.pushParam(CONF_WORKING_POINT, m_WorkingPoint);
    o.pushLineComment("Punkt pracy.");
    o.pushParam(CONF_NONLINEARITY_GAIN, m_NonlinearityGain);
    o.pushLineComment("Wzmocnienie w obszarze poza punktem pracy.");

    return o;
}

}
}
